#include "dcdc.hpp"
#include "tim.h"
#include "stimer.h"
#include "bsp_hrtimer.h"
//only debug
#include "bsp_uart.h"

#define debug_printf(fmt, ...) UARTx_Printf(&huart3, fmt, ##__VA_ARGS__)
#define debug_printn(data, n) UARTx_Printn(&huart3, data, n)

#define alter_trigger()     do{Beep(5000);LEDx_Write(LED_ERR, 1);}while(0)
#define alter_trigger_off() do{LEDx_Write(LED_ERR, 0);}while(0)

dcdcPower dcpower;

dcdcPower::dcdcPower(void)
{
    state = STATE_IDLE;
    sub_state = 0;

    // init
    timeout_wait = DCDC_INIT_TIMEOUT_WAIT;
    dead_time_high = DCDC_INIT_DEAD_HV;
    duty_high = DCDC_INIT_DUTY_HV;
    dead_time_low = DCDC_INIT_DEAD_LV;
    duty_low = DCDC_INIT_DUTY_LV;
    soft_start_time = DCDC_INIT_SOFT_START_TIME;
    soft_start_initFreq_HV = DCDC_INIT_SOFT_START_INIT_FREQ_HV;
    soft_start_targetFreq_HV = DCDC_INIT_SOFT_START_TARGET_FREQ_HV;
    soft_start_initFreq_LV = DCDC_INIT_SOFT_START_INIT_FREQ_LV;
    soft_start_targetFreq_LV = DCDC_INIT_SOFT_START_TARGET_FREQ_LV;
    safe_guard_flag = DCDC_SAFE_GUARD_FLAG_INIT;
    feedback_param_init();
}

void dcdcPower::feedback_param_init(void)
{
    V_h = 0;
    I_h = 0;
    V_l = 0;
    I_l = 0;
    V_target = 0;
    I_target = 0;
    F_current = 0;
    
    // 初始化PID参数
    pid_inc_init(&pid_boost_V,
                    DCDC_BOOST_V_PID_KP_INIT,
                    DCDC_BOOST_V_PID_KI_INIT,
                    DCDC_BOOST_V_PID_KD_INIT);
    pid_inc_init(&pid_boost_I,
                    DCDC_BOOST_I_PID_KP_INIT,
                    DCDC_BOOST_I_PID_KI_INIT,
                    DCDC_BOOST_I_PID_KD_INIT);
    pid_inc_init(&pid_buck_V,
                    DCDC_BUCK_V_PID_KP_INIT,
                    DCDC_BUCK_V_PID_KI_INIT,
                    DCDC_BUCK_V_PID_KD_INIT);
    pid_inc_init(&pid_buck_I,
                    DCDC_BUCK_I_PID_KP_INIT,
                    DCDC_BUCK_I_PID_KI_INIT,
                    DCDC_BUCK_I_PID_KD_INIT);
}

void dcdcPower::start(power_mode_e Mode)
{
    if (this->state != STATE_IDLE)
    {
        // busy
        return;
    }
    this->mode = Mode;
    this->time = 0;
    this->sleep_time = 0;
    this->sub_state = 0;
    this->sub_state_shadow = 0;
    this->state = STATE_WAIT;
    debug_printf("[power] start mode %d\r\n", this->mode);
    sleep(100, 0);
}

void dcdcPower::stop(void)
{
    DCDC_MOSL_DISABLE();
    DCDC_MOSH_DISABLE();
    DCDC_LV_PWM_Stop();
    DCDC_HV_PWM_Stop();
    // if (mode == MODE_BOOST_OPEN || mode == MODE_BOOST_FEEDBACK)
    // {
    // }
    // if (mode == MODE_BUCK_OPEN || mode == MODE_BUCK_FEEDBACK)
    // {
    // }
    debug_printf("[power] stop mode %d\r\n", this->mode);
    this->state = STATE_IDLE;
}

bool dcdcPower::check_work_condition(void)
{
    // TODO: 检查工作条件
    // 检查电压
    if (DCDC_GET_HV() > DCDC_HV_MAX_VOLTAGE ||
        DCDC_GET_LV() > DCDC_LV_MAX_VOLTAGE
        // DCDC_GET_HV() < DCDC_HV_MIN_VOLTAGE ||
        // DCDC_GET_LV() < DCDC_LV_MIN_VOLTAGE
    )
    {
        debug_printf("[power] check_work_condition error: voltage too high\r\n");
        return false;
    }
    // 升压模式检查低侧电压
    if (mode == MODE_BOOST_OPEN || mode == MODE_BOOST_FEEDBACK)
    {
        if (DCDC_GET_LV() < DCDC_LV_MIN_VOLTAGE)
        {
            debug_printf("[power] check_work_condition error: LV too low\r\n");
            return false;
        }
    }
    // 降压模式检查高侧电压
    else if (mode == MODE_BUCK_OPEN || mode == MODE_BUCK_FEEDBACK)
    {
        if (DCDC_GET_HV() < DCDC_HV_MIN_VOLTAGE)
        {
            debug_printf("[power] check_work_condition error: HV too low\r\n");
            return false;
        }
    }
    // 检查电流
    if (DCDC_GET_HI() > DCDC_MAX_SAFE_HI ||
        DCDC_GET_LI() > DCDC_MAX_SAFE_LI )
    {
        debug_printf("[power] check_work_condition error: current\r\n");
        return false;
    }
    // 检查温度
    if (DCDC_GET_T1() > DCDC_MAX_SAFE_TEMP1 ||
        DCDC_GET_T2() > DCDC_MAX_SAFE_TEMP2 ||
        DCDC_GET_T3() > DCDC_MAX_SAFE_TEMP3)
    {
        debug_printf("[power] check_work_condition error: temperature\r\n");
        return false;
    }
    // 检查其他条件
    return true;
}


void dcdcPower::run(void)
{
    switch (this->state)
    {
    case STATE_IDLE:
        // 等待控制命令
        break;
    case STATE_WAIT://等待
        if (sub_state == 0)
        {
            // 检查工作条件
            if (check_work_condition())
            {
                // 进入初始化状态
                this->state = STATE_INIT;
                sleep(100, 0);
                debug_printf("[power] work condition OK\r\n");
            }
            else if (time < timeout_wait)
            {
                // 继续等待
                sleep(100);
            }
            else
            {
                // 等待超时
                this->state = STATE_ERROR;
                sleep(100, 0);
            }
        }
        break;
    case STATE_INIT://初始化
        
        if (sub_state == 0)
        {
            if (mode == MODE_BOOST_OPEN || mode == MODE_BOOST_FEEDBACK)// 开启低侧
            {
                // 设置PWM参数
                DCDC_LV_SETFREQ(this->soft_start_initFreq_LV);
                DCDC_LV_SETDUTY(this->duty_low);
                DCDC_LV_SETDEAD(this->dead_time_low);
                DCDC_LV_PWM_Start();// 开启PWM输出
                DCDC_MOSL_ENABLE();// 开启MOS使能
                // 计算软起动步进频率
                this->soft_start_stepFreq = this->soft_start_initFreq_LV - this->soft_start_targetFreq_LV;
                this->soft_start_stepFreq /= this->soft_start_time;
                this->soft_start_currentFreq = this->soft_start_initFreq_LV;
                debug_printf("[power] soft start time=%dms,Target=%dHz,step=%dHz\r\n",
                this->soft_start_time, this->soft_start_targetFreq_LV, this->soft_start_stepFreq);
            }
            else if (mode == MODE_BUCK_OPEN || mode == MODE_BUCK_FEEDBACK)// 开启高侧
            {
                // 设置PWM参数
                DCDC_HV_SETFREQ(this->soft_start_initFreq_HV);
                DCDC_HV_SETDUTY(this->duty_high);
                DCDC_HV_SETDEAD(this->dead_time_high);
                DCDC_HV_PWM_Start();// 开启PWM输出
                DCDC_MOSH_ENABLE();// 开启MOS使能
                // 计算软起动步进频率
                this->soft_start_stepFreq = this->soft_start_initFreq_HV - this->soft_start_targetFreq_HV;
                this->soft_start_stepFreq /= this->soft_start_time;
                this->soft_start_currentFreq = this->soft_start_initFreq_HV;
                debug_printf("[power] soft start time=%dms,Target=%dHz,step=%dHz\r\n",
                this->soft_start_time, this->soft_start_targetFreq_HV, this->soft_start_stepFreq);
                this->safe_guard_flag = 0;
            }
            sleep(10, 1);
        }
        else if (sub_state == 1)
        {
            /* 软起动 */
            switch (this->mode)
            {
            case MODE_BOOST_OPEN:
            case MODE_BOOST_FEEDBACK:
                /* 低侧处理 */
                // 判断频率是否达到
                if (this->soft_start_currentFreq - this->soft_start_targetFreq_LV > 4000
                 && this->soft_start_currentFreq - this->soft_start_stepFreq > 0)
                {
                    // 降低频率
                    DCDC_LV_SETFREQ(this->soft_start_currentFreq - this->soft_start_stepFreq);
                    this->soft_start_currentFreq -= this->soft_start_stepFreq;
                    sleep(1);
//										debug_printf("[power] soft start current freq=%d\r\n", this->soft_start_currentFreq);
                }
                else
                {
                    // 完成软起动
                    this->safe_guard_flag = 1;
                    DCDC_LV_SETFREQ(this->soft_start_targetFreq_LV);
                    debug_printf("[power] soft start end, current freq=%d\r\n", this->soft_start_targetFreq_LV);
                    sleep(100, 2);
                }
            break;
            case MODE_BUCK_OPEN:
            case MODE_BUCK_FEEDBACK:
                /* 高侧处理 */
                // 判断频率是否达到
                if (this->soft_start_currentFreq - this->soft_start_targetFreq_HV > 4000
                 && this->soft_start_currentFreq - this->soft_start_stepFreq > 0)
                {
                    // 降低频率
                    DCDC_HV_SETFREQ(this->soft_start_currentFreq - this->soft_start_stepFreq);
                    this->soft_start_currentFreq -= this->soft_start_stepFreq;
                    sleep(1);
                    // debug_printf("[power] soft start current freq=%d\r\n", this->soft_start_currentFreq);
                }
                else
                {
                    // 完成软起动
                    DCDC_HV_SETFREQ(this->soft_start_targetFreq_HV);
                    debug_printf("[power] soft start end, current freq=%d\r\n", this->soft_start_targetFreq_HV);
                    sleep(100, 2);
                }
            break;
            default:
                break;
            }
        }
        else if (sub_state == 2)
        {
            this->state = STATE_RUN;
            sleep(100, 0);
        }
        break;
    case STATE_RUN://运行
        // 开环降压模式
        // 闭环降压模式
        // 开环升压模式
        // 闭环升压模式
        sleep(100);
        break;
    case STATE_STOP://停止
        // 关闭PWM
        // 关闭MOS使能
        stop();
        break;
    case STATE_ERROR://错误
        // 启动超时
        // 过压
        // 欠压
        // 过流
        // 过温
        break;
    default:
        break;
    }
}


void dcdcPower::wake_task(const void * arg)
{
    dcdcPower *object = (dcdcPower*)arg;
    object->time += object->sleep_time;
    object->sleep_time = 0;
    if (object->sub_state_shadow != 0xFF)
    {
        object->sub_state = object->sub_state_shadow;
    }
    object->sub_state_shadow = 0;
    // debug_printf("[power] wake up task\n\r");
    object->run();
}

/**
 * @brief 休眠一段时间
 * @param ms 时间
 * @param subState 休眠后更新子状态的值[0,255)
 * @retval true 休眠成功
 * @retval false 当前休眠未结束
 */
bool dcdcPower::sleep(uint32_t ms, uint8_t subState)
{
    if (sleep_time != 0)
    {
        return 0;// 当前休眠未结束
    }
    if (ms == 0) ms = 1;
    sleep_time = ms;// 开启休眠
    sub_state_shadow = subState;
    stimer_task_oneshot(wake_task, sleep_time, 1, this);
    return 1;
}

bool dcdcPower::sleep(uint32_t ms)
{
    return sleep(ms, 0xFF); // 0xFF用做不更新子状态设置的标志
}

bool dcdcPower::awake(void)
{
    // debug_printf("currnt state:%d,next:%d\n\r", state, sub_state);
    sleep_time = 0;
    return sleep(1);
}

bool dcdcPower::awake(uint8_t substate)
{
    // debug_printf("currnt state:%d,next:%d\n\r", state, substate);
    sleep_time = 0;
    return sleep(1, substate);
}

void dcdcPower::reflash_input(void)
{
    this->V_h = DCDC_GET_HV();
    this->I_h = DCDC_GET_HI(); // - 输入
    this->V_l = DCDC_GET_LV();
    this->I_l = DCDC_GET_LI(); // - 输出
    if (safe_guard_flag) // 开启安全守护标志
    {
        safe_guard_serve();
    }
}


void dcdcPower::feedback_boost_start(void)
{
    // 开启定时器中断
    __HAL_TIM_ENABLE_IT(&htim15, TIM_IT_UPDATE);
}

void dcdcPower::feedback_boost_stop(void)
{
    // 关闭定时器中断
    __HAL_TIM_DISABLE_IT(&htim15, TIM_IT_UPDATE);
}

void dcdcPower::feedback_buck_start(void)
{
    // 开启定时器中断
    __HAL_TIM_ENABLE_IT(&htim15, TIM_IT_UPDATE);
}

void dcdcPower::feedback_buck_stop(void)
{
    // 关闭定时器中断
    __HAL_TIM_DISABLE_IT(&htim15, TIM_IT_UPDATE);
}

// 降压控制循环
void dcdcPower::buck_regulator_loop(void)
{
    /* 电压环 */
    this->F_current -= this->pid_buck_V.Kp*(this->V_target - this->V_l);
    // pid_inc_calculate(&this->pid_buck_V,
    //                                      this->V_target,
    //                                      this->V_l);
    // 输出限制
    if (this->F_current >= DCDC_HV_MAX_FREQ)
    {
        this->F_current = DCDC_HV_MAX_FREQ;
    }
    else if (this->F_current <= DCDC_HV_MIN_FREQ)
    {
        this->F_current = DCDC_HV_MIN_FREQ;
    }
    DCDC_HV_SETFREQ(this->F_current);
}

// 升压控制循环
void dcdcPower::boost_regulator_loop(void)
{
    // TODO:
    LEDx_Togger(LED_ERR);
}

void dcdcPower::ex_regulator_serve(void)
{
    if (state != STATE_RUN)
    {
        return;
    }
    switch (mode)
    {
    case MODE_BUCK_FEEDBACK:
        buck_regulator_loop();
        break;
    case MODE_BOOST_FEEDBACK:
        boost_regulator_loop();
        break;
    default:
        break;
    }
}

/**
 * @brief 设置降压模式的闭环电压
 * @param valtage DCDC_LV_MIN_VOLTAGE ~ DCDC_LV_MAX_VOLTAGE (V)
 */
void dcdcPower::set_buck_voltage(float valtage)
{
    if (valtage > DCDC_LV_MAX_VOLTAGE)
    {
        valtage = DCDC_LV_MAX_VOLTAGE;
    }
    else if (valtage < DCDC_LV_MIN_VOLTAGE)
    {
        valtage = DCDC_LV_MIN_VOLTAGE;
    }
    this->V_target = valtage;
}

/**
 * @brief 风扇初始化
 */
void dcdcPower::fan_init(void)
{
//    HAL_TIM_Base_Start(&htim17);
//    HAL_TIM_Base_Start(&htim16);

    set_fan_speed(DCDC_FAN_ID0, 0);
    set_fan_speed(DCDC_FAN_ID1, 0);
    
    if (DCDC_AUTO_FAN_INIT == 1)
    {
        fan_auto_start();
    }
}

/**
 * @brief 风扇自动控制服务函数
 * @param arg 
 */
void dcdcPower::fan_serve(const void *arg)
{
    if (arg == NULL) return;
    dcdcPower *object = (dcdcPower *)arg;
    if (DCDC_GET_T1() > DCDC_FAN_AUTO_START_TEMP ||
		DCDC_GET_T2() > DCDC_FAN_AUTO_START_TEMP ||
        DCDC_GET_T3() > DCDC_FAN_AUTO_START_TEMP)
    {
        // 温度过高，风扇全速
        object->set_fan_speed(DCDC_FAN_ID0, 1000);
        object->set_fan_speed(DCDC_FAN_ID1, 1000);
    }
    else if (DCDC_GET_T1() < DCDC_FAN_AUTO_STOP_TEMP &&
		     DCDC_GET_T2() < DCDC_FAN_AUTO_STOP_TEMP &&
             DCDC_GET_T3() < DCDC_FAN_AUTO_STOP_TEMP)
    {
        object->set_fan_speed(DCDC_FAN_ID0, 0);
        object->set_fan_speed(DCDC_FAN_ID1, 0);
    }
    if (object->fanSpeedAuto_flag == 1)
    {
        stimer_task_set_repetitions(hstimer.wait_id, 1);
    }

}

/**
 * @brief 启动风扇自动控制
 */
void dcdcPower::fan_auto_start(void)
{
    fanSpeedAuto_flag = 1;
    stimer_task_oneshot(fan_serve, 1000, 0, this);
}
/**
 * @brief 停止风扇自动控制
 */
void dcdcPower::fan_auto_stop(void)
{
    fanSpeedAuto_flag = 0;
}
/**
 * @brief 设置风扇速度
 * @param id 风扇ID
 * @param speed 风扇速度 0 ~ 1000
 */
void dcdcPower::set_fan_speed(uint8_t id, uint16_t speed)
{
    uint16_t compareValue = speed > 1000 ? 1000 : speed;
    switch (id)
    {
    case DCDC_FAN_ID0:
		  if (compareValue > 0) Pinx_RESET(Fan1);
		  else Pinx_SET(Fan1);
//        __HAL_TIM_SET_COMPARE(&htim17, TIM_CHANNEL_1, compareValue); //PB9 Fan1 TIM17
//        if (speed == 0) HAL_TIM_PWM_Stop(&htim17, TIM_CHANNEL_1);
//        else HAL_TIM_PWM_Start(&htim17, TIM_CHANNEL_1);
        break;
    case DCDC_FAN_ID1:
		  if (compareValue > 0) Pinx_RESET(Fan2);
		  else Pinx_SET(Fan2);
//        __HAL_TIM_SET_COMPARE(&htim16, TIM_CHANNEL_1, compareValue); //PB8 Fan2 TIM16
//        if (speed == 1) HAL_TIM_PWM_Stop(&htim16, TIM_CHANNEL_1);
//        else HAL_TIM_PWM_Start(&htim16, TIM_CHANNEL_1);
        break;
    default:
        // error id
        break;
    }
}

/**
 * @brief 安全保护服务函数
 */
void dcdcPower::safe_guard_serve(void)
{
    /* 电压电流保护 */
    if (this->V_h > DCDC_HV_MAX_VOLTAGE ||
        this->V_l > DCDC_LV_MAX_VOLTAGE ||
        this->I_h > DCDC_HV_MAX_CURRENT ||
        this->I_l > DCDC_LV_MAX_CURRENT)
    {
        dcpower.stop();
        alter_trigger();
        
    }
    else
    {
        alter_trigger_off();
    }
}


